#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>

using namespace std;
using namespace Eigen;

int main()
{
    //旋转矩阵
    Matrix3d rotation_matrix = Matrix3d::Identity();
    AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1));  //表示沿Z轴旋转45度
    rotation_matrix = rotation_vector.toRotationMatrix();

    cout<<"R: "<<endl<<rotation_matrix<<endl<<endl;

    //构造对应的四元数
    Eigen::Quaterniond q(rotation_matrix);
    cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;

    //使用对数映射获得它的李代数
    Sophus::SO3d SO3_R(rotation_matrix);
    cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;

    //更新量omega
    Eigen::Vector3d omega(0.01, 0.02, 0.03); //更新量

    // 李代数更新
    Matrix3d skew_symmetric_matrix = Sophus::SO3d::hat(omega).matrix();
    cout<<"SO3 hat= "<<endl<< skew_symmetric_matrix<<endl<<endl;
    Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(omega);
    cout<<"SO3 updated = "<<endl<<(SO3_R * Sophus::SO3d::exp(omega)).matrix()<<endl<<endl;

    // 四元数更新
    Eigen::Quaterniond q_update(1, omega(0)/2, omega(1)/2, omega(2)/2);
    Eigen::Quaterniond q_updated = q * q_update.normalized();
    cout<<"Quaterniond updated = "<< endl << q_updated.toRotationMatrix() <<endl<<endl;

    //衡量两种方法之间的差距
    cout<<"disparity = "<< endl <<SO3_updated.matrix().transpose()*q_updated.toRotationMatrix()<<endl;
}

